Unterrichtsbeispiel#
Lernziele#
Sie erkennen, dass viele mathematische Problemstellungen mittels Vektorrechnung einfacher gelöst werden können.
Sie beherrschen die wichtigsten Vektoroperationen (Addition/Subtraktion, Skalarprodukt, Vektorprodukt, Drehungen/Koordinatentransformationen).
Sie können die Vektorrechnung an einem einfachen Roboter korrekt anwenden.
Aufgabe#
Wir wollen die Bewegung eines Roboterarms mit zwei Drehgelenken (zwei Freiheitsgrade) berechnen, wobei wir mit Vektoren arbeiten wollen. Gesucht ist die Position des Greifers (End Effector) in Abhängigkeit der Gelenkwinkel \(\varphi_1\) und \(\varphi_2\).
Gegeben sind die Längen \(l_1\) und \(l_2\) der beiden Arme sowie der Zeitverlauf der Gelenkwinkel in einem Textfile angles.txt.
import numpy as np
import matplotlib.pyplot as plt
l_1 = 1 # Länge [m] des ersten Arms
l_2 = 0.5 # Länge [m] des zweiten Arms
p_1_0 = np.array([l_1, 0]) # Vektor p_1 in Nullstellung (phi_1 = 0)
p_2_0 = np.array([l_2, 0]) # Vektor p_2 in Nullstellung (phi_2 = 0)
a) Drehwinkel einlesen#
Die Drehwinkel \(\varphi_1\) bzw. \(\varphi_2\) sind im Textfile angles.txt abgelegt. Dieses soll mit NumPy eingelesen werden und entsprechenden Variablen zugewiesen werden.
data = np.loadtxt('data/angles.txt') # 2D-Array
time = data[:, 0] # 1D-Array
phi_1 = data[:, 1] # "
phi_2 = data[:, 2] # "
N = len(time) # Anzahl Zeitschritte bzw. Winkelschritte
b) Drehmatrix aufstellen#
Die Rotationsmatrix in 2D ist eine 2x2-Matrix mit entsprechenden Einträgen (\(\cos(\varphi), \sin(\varphi)\) etc., vgl. Theorienotebook) und soll hier als Funktion R(phi) definiert werden. Die Drehung eines Vektors \(\vec p\) erfolgt dann durch die Matrixmultiplikation \(R(\varphi) \cdot \vec p\).
def R(phi): # 2D-Drehmatrix
return np.array([[np.cos(phi), -np.sin(phi)],
[np.sin(phi), np.cos(phi)]])
c) Berechnung der Greiferposition#
Die Positionen des Gelenkes \(\vec{p}_1\) und des Greifers \(\vec{p}_2\) sollen für alle Zeitpunkte bzw. Gelenkwinkel \(\varphi_1\) bzw. \(\varphi_2\) berechnet werden.
p_1 = np.zeros((N, 2)) # Array für Gelenkpos. initialisieren
p_2 = np.zeros((N, 2)) # Array für Greiferpos. initialisieren
for i in range(N):
p_1[i] = R(phi_1[i]) @ p_1_0 # Gelenkpos. (@-Operator = Matrixmultiplikation)
# p_1[i] = np.matmul(R(phi_1[i]), p_1_0) # alternativ mit np.matmul()
p_2[i] = p_1[i] + R(phi_1[i]) @ R(phi_2[i]) @ p_2_0 # Greiferpos.
# Alternativ mit List-Comprehensions
# p_1 = np.array([R(phi) @ p_1_0 for phi in phi_1])
# p_2 = p_1 + np.array([R(phi1) @ R(phi2) @ p_2_0 for phi1, phi2 in zip(phi_1, phi_2)])
d) Grafische Darstellung des Roboterarms#
Der Positionsverlauf des Greifers soll grafisch in einem Diagramm dargestellt werden. Der Roboterarm soll dabei in der Grundposition (Position zum Zeitpunkt 0) dargestellt werden.
fig, ax = plt.subplots()
ax.plot(p_2[:,0], p_2[:,1], 'b', label='Trajektorie')
line, = ax.plot([0, p_1[0, 0], p_2[0, 0]], [0, p_1[0, 1], p_2[0, 1]], 'ok-', label='Roboterarm')
ax.set_title('Roboterbewegung')
ax.set_xlabel('x [m]')
ax.set_ylabel('y [m]')
ax.grid()
ax.legend()
plt.show()
Animation der Roboterbewegung#
import matplotlib.animation as animation
def update(frame): # in dieser Funktion wird Plot aktualisiert (frame für frame)
line.set_data([0, p_1[frame, 0], p_2[frame, 0]], [0, p_1[frame, 1], p_2[frame, 1]])
return line
ani = animation.FuncAnimation(fig, update, interval=100, frames=N) # 2. Argument ist obige update()-Funktion
ani.save('Robot.gif') # Animation als GIF speichern
plt.show() # In PyCharm/VS Code wird damit die Animation angezeigt (aber nicht in Jupyterlab)
from IPython.display import HTML
HTML(ani.to_jshtml()) # Animation direkt in Jupyterlab ausgeben
Angewandte Mathematik | MAT_AGW | 2024